/*
 * AffineRansac.cpp
 *
 *  Created on: Aug 31, 2012
 *      Author: alberto
 */

#include "AffineRansac.h"

AffineRansac::AffineRansac(const vector<KeyPoint>& kA, const vector<KeyPoint>& kB, const vector<DMatch>& m): delta(60), iterations(15) {
	vector<Point2f> pA;
	vector<Point2f> pB;

	KeyPoint::convert(kA, pA);
	KeyPoint::convert(kB, pB);

	Mat auxA = Mat(pA).reshape(1, pA.size());
	Mat auxB = Mat(pB).reshape(1, pB.size());

	auxA.copyTo(pointsA);
	auxB.copyTo(pointsB);

	matches = m;
        int k =1;
	return;

}

AffineRansac::~AffineRansac() {
	// TODO Auto-generated destructor stub
}

void AffineRansac::setDelta(double d){ delta = d; }

int AffineRansac::calculateIterations(double threshold, int samples, double outlierRate){

	double it = log10(1 - threshold)/log10((1 - pow((1 - outlierRate), samples)));
	return ceil(it);
}

double AffineRansac::testTransform(const Mat& affineTransform){

	size_t nm = matches.size();
	int inliers = 0;
	int outliers = 0;
	double inlierRate;
	double outlierRate;

	Mat tempAffine;
	Mat tempAPoints;
	Mat aux;
	Mat warpedAPoints;

	affineTransform.convertTo(tempAffine, pointsA.type());
	tempAPoints = Mat::ones(3, pointsA.rows, pointsA.type());
	aux = pointsA.t();
	aux.copyTo(tempAPoints.rowRange(0, 2));

	warpedAPoints = tempAffine * tempAPoints;

	for(size_t i=0; i < nm; i++){
	    int idxA = matches[i].queryIdx;
	    int idxB = matches[i].trainIdx;

	    Mat ptA = warpedAPoints.col(idxA).t();
	    Mat ptB = pointsB.row(idxB);

	    Mat r = ptA - ptB;
	    Mat Mag;
	    magnitude(r.col(0), r.col(1), Mag);
	    float dist = Mag.at<float>(0);

	    if(dist < delta) inliers++;
	    else outliers++;
	}

	return outliers/(float)nm;
}

Mat AffineRansac::solve(){

	Mat estAffineTransf;
	Mat bestAffineTransf;
	Point2f choosenAPoints[3];
	Point2f choosenBPoints[3];
	int *indexes;
	double outlierRate;
	double bestOutlierRate;
	double time;

	int it = 0;
	int maxIt = 0;
	int totalIt = 0;
	int totalUpdates = 0;


	// Run the first iteration of RANSAC to measure the max number of iterations until converging
	cout << "Starting RANSAC" << endl;
	time = (double)getTickCount();
	indexes = generateRandomIndexes(matches.size());
	for(int i=0; i<3; i++){

		size_t ipntA = matches[indexes[i]].queryIdx;
		size_t ipntB = matches[indexes[i]].trainIdx;

		choosenAPoints[i] = Point2f(pointsA.at<float>(ipntA, 0), pointsA.at<float>(ipntA, 1));
		choosenBPoints[i] = Point2f(pointsB.at<float>(ipntB, 0), pointsB.at<float>(ipntB, 1));
	}

	estAffineTransf = getAffineTransform(choosenAPoints, choosenBPoints);
	bestAffineTransf = estAffineTransf;

	outlierRate = testTransform(estAffineTransf);
	bestOutlierRate = outlierRate;

	maxIt = calculateIterations(0.99, 3, outlierRate);

	while(it < maxIt && it < 100 && totalIt < 1000){
		indexes = generateRandomIndexes(matches.size());
		for(int i=0; i<3; i++){

			size_t ipntA = matches[indexes[i]].queryIdx;
			size_t ipntB = matches[indexes[i]].trainIdx;

			choosenAPoints[i] = Point2f(pointsA.at<float>(ipntA, 0), pointsA.at<float>(ipntA, 1));
			choosenBPoints[i] = Point2f(pointsB.at<float>(ipntB, 0), pointsB.at<float>(ipntB, 1));
		}
		estAffineTransf = getAffineTransform(choosenAPoints, choosenBPoints);
		outlierRate = testTransform(estAffineTransf);
		if(outlierRate < bestOutlierRate){
			bestOutlierRate = outlierRate;
			bestAffineTransf = estAffineTransf;
			maxIt = calculateIterations(0.99, 3, outlierRate);
			it = 0;
			totalUpdates++;
		}

		it++;
		totalIt++;
		if((it == maxIt || it == 100) && bestOutlierRate > 0.9){
			it = 0;
		}

	}
	time = ((double)getTickCount() - time)/getTickFrequency();
	cout << "done RANSAC" << endl;
	cout << "Minimal Outlier Rate: " << bestOutlierRate << endl;
	cout << "Total iterations: " << totalIt << endl;
	cout << "Total transformation updates: " << totalUpdates << endl;
	cout << "Total time: " << time << "s" << endl;

	return bestAffineTransf;
}

